#ifndef BTL_SLAM_MAP
#define BTL_SLAM_MAP

#include <btl/Image/Image.hpp>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace btl
{
namespace slam
{

   /// Everything in a map has a unique ID number to identify it
   /// this is useful for:
   ///   - Serialisation
   ///   - Versioned/snapshot-based map structures
   ///   - Quick display of identity in the UI
   /// and possibly other things (generally, anywhere that you want to
   /// be able to uniquely identify a particular item in the map,
   /// without requiring a persistent memory address)
   class MapItem
   {
      public:
         const unsigned int id;
   };

   class MapNode : public MapItem
   {
      public:
         virtual ~MapNode() {}
   };

   class MapLink : public MapItem
   {
      public:
         virtual ~MapLink() {}

         virtual MapNode* getFrom() { return _pFrom; }
         virtual MapNode* getTo() { return _pTo; }

      protected:
         MapNode* _pFrom;
         MapNode* _pTo;
   };

   class CameraView : public MapNode
   {
      public:
         btl::ImageRegionConstRGB getImage() const { return _image; }

      protected:
         btl::ImageRGB _image;
   };

   class Landmark : public MapNode
   {
   };

   template <typename DescriptorType>
   class LandmarkPoint : public Landmark
   {
      public:
         typedef DescriptorType descriptor_type;

         const descriptor_type& getDescriptor() const { return _descriptor; }

      protected:
         descriptor_type _descriptor;
   };

   class MapLinkTransform : public MapLink
   {
      public:
         virtual Eigen::Affine3d getTransform() const = 0;
   };

   class MapLinkTranslation : public MapLinkTransform
   {
      public:
         virtual Eigen::Affine3d getTransform() const
         { return Eigen::Affine3d(Eigen::Translation3d(_translation)); }

      protected:
         Eigen::Vector3d _translation;
   };

   class MapLinkRotationTranslation : public MapLinkTransform
   {
      public:
         virtual Eigen::Affine3d getTransform() const
         { return _transform; }

      protected:
         Eigen::Affine3d _transform;
   };

   /// Note: maps can be embedded inside other maps, which allows
   /// you to implement things like EKF-SLAM submapping schemes more
   /// naturally (and, e.g., a topological map containing a graph of
   /// metric maps)
   class Map : public MapNode
   {
      public:
         //virtual void visitAllNodes( visitor(node) );
         //virtual void visitAllLinks( visitor(link) );

         //template <typename ItemType>
         //void visitNodesOfType( visitor(node) );

         //virtual void visitNodesFrom( rootNode, visitor(link, node) );
   };

} // namespace slam
} // namespace btl


// ====================================================================
// === Implementation


namespace btl
{
namespace slam
{

} // namespace slam
} // namespace btl

#endif // BTL_SLAM_MAP
